clc
clear
close all
syms a b c m I g %konstanty
syms dy dpsi psi dx deltaf deltar alpha s mu Fz X Y alfa
%8a
vyfl=symfun(dy+a*dpsi, [dy, dpsi]);
vxfl=symfun(dx-c*dpsi, [dx, dpsi]);
%8b
vyfr=symfun(dy+a*dpsi, [dy, dpsi]);
vxfr=symfun(dx+c*dpsi, [dx, dpsi]);
%8c
vyrl=symfun(dy-b*dpsi, [dy, dpsi]);
vxrl=symfun(dx-c*dpsi, [dx, dpsi]);
%8d
vyrr=symfun(dy-b*dpsi, [dy, dpsi]);
vxrr=symfun(dx+c*dpsi, [dx, dpsi]);

first={'f','r'};
second={'l','r'};

for i=1:2;
    for j=1:2;
        %7a
        eval(['vc',cell2mat(first(i)),cell2mat(second(j)),'=symfun(vy',cell2mat(first(i)),cell2mat(second(j)),'(dy,dpsi)*cos(pi/180*delta',cell2mat(first(i)),')-vx',cell2mat(first(i)),cell2mat(second(j)),'(dx,dpsi)*sin(pi/180*delta',cell2mat(first(i)),'),[dx, dy, dpsi, deltaf, deltar])']);
        %7b
        eval(['vl',cell2mat(first(i)),cell2mat(second(j)),'=symfun(vy',cell2mat(first(i)),cell2mat(second(j)),'(dy,dpsi)*sin(pi/180*delta',cell2mat(first(i)),')+vx',cell2mat(first(i)),cell2mat(second(j)),'(dx,dpsi)*cos(pi/180*delta',cell2mat(first(i)),'),[dx, dy, dpsi, deltaf, deltar])']);
    end
end

for i=1:2;
    for j=1:2;
        %6
        eval(['alpha',cell2mat(first(i)),cell2mat(second(j)),'=symfun(atan(vc',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar)/vl',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar)),[dx, dy, dpsi, deltaf, deltar])']);
    end
end

fc(alfa,s,mu,Fz)=tyre_fc();
fl(alfa,s,mu,Fz)=tyre_fl();

for i=1:2;
    for j=1:2;
        %definice konstant
        eval(['syms mu',cell2mat(first(i)),cell2mat(second(j)),' Fz',cell2mat(first(i)),cell2mat(second(j)),' s',cell2mat(first(i)),cell2mat(second(j))]);
        %4a
        eval(['Fc',cell2mat(first(i)),cell2mat(second(j)),'=symfun(fc(alpha',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar),s',cell2mat(first(i)),cell2mat(second(j)),',mu',cell2mat(first(i)),cell2mat(second(j)),',Fz',cell2mat(first(i)),cell2mat(second(j)),'),[dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),',mu',cell2mat(first(i)),cell2mat(second(j)),',Fz',cell2mat(first(i)),cell2mat(second(j)),'])']);
        %4b
        eval(['Fl',cell2mat(first(i)),cell2mat(second(j)),'=symfun(fl(alpha',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar),s',cell2mat(first(i)),cell2mat(second(j)),',mu',cell2mat(first(i)),cell2mat(second(j)),',Fz',cell2mat(first(i)),cell2mat(second(j)),'),[dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),',mu',cell2mat(first(i)),cell2mat(second(j)),',Fz',cell2mat(first(i)),cell2mat(second(j)),'])']);
    end
end

for i=1:2;
    for j=1:2;
        %3a
        eval(['Fy',cell2mat(first(i)),cell2mat(second(j)),'=symfun(Fl',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),', mu',cell2mat(first(i)),cell2mat(second(j)),', Fz',cell2mat(first(i)),cell2mat(second(j)),')*sin(pi/180*delta',cell2mat(first(i)),')+Fc',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),', mu',cell2mat(first(i)),cell2mat(second(j)),', Fz',cell2mat(first(i)),cell2mat(second(j)),')*cos(pi/180*delta',cell2mat(first(i)),'),[dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),',mu',cell2mat(first(i)),cell2mat(second(j)),',Fz',cell2mat(first(i)),cell2mat(second(j)),'])']);
        %3b
        eval(['Fx',cell2mat(first(i)),cell2mat(second(j)),'=symfun(Fl',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),', mu',cell2mat(first(i)),cell2mat(second(j)),', Fz',cell2mat(first(i)),cell2mat(second(j)),')*cos(pi/180*delta',cell2mat(first(i)),')-Fc',cell2mat(first(i)),cell2mat(second(j)),'(dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),', mu',cell2mat(first(i)),cell2mat(second(j)),', Fz',cell2mat(first(i)),cell2mat(second(j)),')*sin(pi/180*delta',cell2mat(first(i)),'),[dx, dy, dpsi, deltaf, deltar, s',cell2mat(first(i)),cell2mat(second(j)),',mu',cell2mat(first(i)),cell2mat(second(j)),',Fz',cell2mat(first(i)),cell2mat(second(j)),'])']);
    end
end

Fxfl=simplify(Fxfl);
Fxfr=simplify(Fxfr);
Fxrl=simplify(Fxrl);
Fxrr=simplify(Fxrr);

Fyfl=simplify(Fyfl);
Fyfr=simplify(Fyfr);
Fyrl=simplify(Fyrl);
Fyrr=simplify(Fyrr);

Fzfl=symfun(b*m*g/(2*(a+b))/1000,[a,b,m,g]);
Fzfr=symfun(b*m*g/(2*(a+b))/1000,[a,b,m,g]);
Fzrl=symfun(a*m*g/(2*(a+b))/1000,[a,b,m,g]);
Fzrr=symfun(a*m*g/(2*(a+b))/1000,[a,b,m,g]);

fce=[-dx*dpsi+(Fyfl(dx, dy, dpsi, deltaf, deltar, sfl, mufl, Fzfl(a,b,m,g))+Fyfr(dx, dy, dpsi, deltaf, deltar, sfr, mufr, Fzfr(a,b,m,g))+Fyrl(dx, dy, dpsi, deltaf, deltar, srl, murl, Fzrl(a,b,m,g))+Fyrr(dx, dy, dpsi, deltaf, deltar, srr, murr, Fzrr(a,b,m,g)))/m;...
    dy*dpsi+(Fxfl(dx, dy, dpsi, deltaf, deltar, sfl, mufl, Fzfl(a,b,m,g))+Fxfr(dx, dy, dpsi, deltaf, deltar, sfr, mufr, Fzfr(a,b,m,g))+Fxrl(dx, dy, dpsi, deltaf, deltar, srl, murl, Fzrl(a,b,m,g))+Fxrr(dx, dy, dpsi, deltaf, deltar, srr, murr, Fzrr(a,b,m,g)))/m;...
    dpsi;...
    (a*(Fyfl(dx, dy, dpsi, deltaf, deltar, sfl, mufl, Fzfl(a,b,m,g))+Fyfr(dx, dy, dpsi, deltaf, deltar, sfr, mufr, Fzfr(a,b,m,g)))-b*(Fyrl(dx, dy, dpsi, deltaf, deltar, srl, murl, Fzrl(a,b,m,g))+Fyrr(dx, dy, dpsi, deltaf, deltar, srr, murr, Fzrr(a,b,m,g)))+c*(-Fxfl(dx, dy, dpsi, deltaf, deltar, sfl, mufl, Fzfl(a,b,m,g))+Fxfr(dx, dy, dpsi, deltaf, deltar, sfr, mufr, Fzfr(a,b,m,g))-Fxrl(dx, dy, dpsi, deltaf, deltar, srl, murl, Fzrl(a,b,m,g))+Fxrr(dx, dy, dpsi, deltaf, deltar, srr, murr, Fzrr(a,b,m,g))))/I;...
    dx*sin(psi)+dy*cos(psi);...
    dx*cos(psi)-dy*sin(psi)];

A=jacobian(fce,[dy;dx;psi;dpsi;Y;X]);

B=jacobian(fce,[deltaf;sfl;sfr;srl;srr]);

matlabFunction(A,'file','carA');
matlabFunction(B,'file','carB');
matlabFunction(fce,'file','fce');

% fileId=fopen('maticeA.txt','w');
% fprintf(fileId,'%s',char(A));
% fclose(fileId);
% 
% fileId=fopen('maticeB.txt','w');
% fprintf(fileId,'%s',char(B));
% fclose(fileId);
% 
% fileID=fopen('fce.tex','w');
% fprintf(fileId,'$$ %s $$',latex(fce));
% fclose(fileId);
% 
% fileID=fopen('maticeA.tex','w');
% fprintf(fileId,'$$ %s $$',latex(A));
% fclose(fileId);
% 
% fileID=fopen('maticeB.tex','w');
% fprintf(fileId,'$$ %s $$',latex(B));
% fclose(fileId);

copyfile('./carA.m','./../simulink model/carA.m');
copyfile('./carB.m','./../simulink model/carB.m');

copyfile('./carA.m','./../model/carA.m');
copyfile('./carB.m','./../model/carB.m');
copyfile('./fce.m','./../model/fce.m');

% copyfile('./maticeA.tex','./text/odvozeni/maticeA.tex');
% copyfile('./maticeB.tex','./text/odvozeni/maticeB.tex');